package com.suray.rgv.fork.core;

import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.utils.Action;
import com.suray.basic.wcs.utils.LoggerType;
import com.suray.basic.wcs.utils.ModuleType;
import com.suray.basic.wcs.utils.WcsFuncUtil;
import com.suray.commin.core.utils.UUID;
import com.suray.rgv.data.Rgv60InfoData;
import com.suray.rgv.data.RgvForkInfoData;
import com.suray.rgv.fork.util.RgvForkLog;
import com.suray.rgv60.core.Rgv60Core;
import com.suray.rgv60.util.RobotIOType;
import com.suray.type.Rgv60TaskStatus;
import com.suray.wcs.res.enums.Order;
import suray.rgv.communication.data.AppiontRout;
import suray.rgv.communication.data.RouteInfo;
import suray.rgv.communication.util.RgvOperation;

import java.io.IOException;
import java.util.Map;

public class RgvForkCore extends Rgv60Core {

    private RgvForkInfo rgvForkInfo;

    public RgvForkCore(int rgvNo, String serialNum) {
        super(rgvNo, serialNum, ModuleType.RGV_FORK);

        rgvForkInfo = new RgvForkInfo(rgv60Info);
    }

    @Override
    public boolean action(Order rgvActionType, Object param) throws IOException {
        switch (rgvActionType) {
            case FORK_UP:
                forkUp(param);
                break;
            case FORK_GET_CARGO:
                forkGetCargo(param);
                break;
            case FORK_DOWN:
                forkDown();
                break;
            case FORK_PUT_CARGO:
                forkPutCargo(param);
                break;
            case FORK_LIFT:
                break;
            case PALLET_UP:
            case PALLET_DOWN:
                throw new RuntimeException("错误的叉车指令:" + rgvActionType);
            default:
                return super.action(rgvActionType, param);
        }

        return true;
    }

    /**
     * 货叉顶升
     * @param param
     */
    private void forkUp(Object param) throws IOException {
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉顶升", "开始，高度：" + param, this.getClass());
        forkAction(generalForkActionRouteInfo((Float) param), "货叉顶升");
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉顶升", "结束", this.getClass());
    }

    /**
     * 货叉取货
     * @param param
     */
    private void forkGetCargo(Object param) throws IOException {
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉取货", "开始，高度：" + param, this.getClass());
        forkAction(generalForkActionRouteInfo((Float) param + (rgvForkInfo.getForkLiftDis())),
                "货叉取货");
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉取货", "结束", this.getClass());
    }

    /**
     * 货叉下降
     */
    private void forkDown() throws IOException {
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉下降", "开始", this.getClass());
        forkAction(generalForkActionRouteInfo(rgvForkInfo.getForkLiftDownHeight()), "货叉下降");
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉下降", "结束", this.getClass());
    }

    /**
     * 货叉放货
     * @param param
     */
    private void forkPutCargo(Object param) throws IOException {
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉放货", "开始，高度：" + param, this.getClass());
        forkAction(generalForkActionRouteInfo((Float) param), "货叉放货");
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉放货", "结束", this.getClass());
    }

    /**
     * 货叉升降
     * @param param
     */
    private void forkLift(Object param) throws IOException {
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉升降", "开始，高度：" + param, this.getClass());
        forkAction(generalForkActionRouteInfo((Float) param), "货叉升降");
        RgvForkLog.info(LoggerType.ACTION, serialNum, "货叉升降", "结束", this.getClass());
    }

    /**
     * 生成货叉动作交互数据
     * @param height
     * @return
     */
    private RouteInfo generalForkActionRouteInfo(Float height) {
        RouteInfo routeInfo = new RouteInfo();
        String taskId = UUID.randomUUID().toString();
//        routeInfo.setId(rgv60Info.getCurrentStation());
        routeInfo.setTask_id(taskId);
        routeInfo.setOperation(RgvOperation.ForkHeight.getStrValue());
        routeInfo.setEnd_height(height);
        return routeInfo;
    }

    @Override
    protected void analyzeNodeAction(AppiontRout.MoveTaskListDTO moveTaskListDTO, Node startNode, Node endNode) {
        for (Action action : endNode.actions) {
            switch (action) {
                case FORK_UP_BY_MOVING:
                    moveTaskListDTO.setFork_mid_height(endNode.getForkHeight());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "行驶时货叉顶升", endNode.print(), this.getClass());
                    break;
                case FORK_UP:
                    moveTaskListDTO.setEnd_height(endNode.getForkHeight());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉顶升", endNode.print(), this.getClass());
                    break;
                case FORK_DOWN_BY_MOVING:
                    moveTaskListDTO.setFork_mid_height(rgvForkInfo.getForkLiftDownHeight());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "行驶时货叉下降", endNode.print(), this.getClass());
                    break;
                case FORK_DOWN:
                    moveTaskListDTO.setEnd_height(rgvForkInfo.getForkLiftDownHeight());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉下降", endNode.print(), this.getClass());
                    break;
                case FORK_GET_CARGO:
                    moveTaskListDTO.setEnd_height(endNode.getForkHeight()
                            + rgvForkInfo.getForkLiftDis());
                    moveTaskListDTO.setOperation(RgvOperation.ForkLoad.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉取货", endNode.print(), this.getClass());
                    break;
                case FORK_PUT_CARGO:
                    moveTaskListDTO.setStart_height(endNode.getForkHeight() + rgvForkInfo.getForkLiftDis());
                    moveTaskListDTO.setEnd_height(endNode.getForkHeight());
                    moveTaskListDTO.setOperation(RgvOperation.ForkUnload.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉放货", endNode.print(), this.getClass());
                    break;
                case FORK_UP_AND_GET_CARGO:
                    moveTaskListDTO.setStart_height(startNode.getForkHeight());
                    moveTaskListDTO.setEnd_height(endNode.getForkHeight() + rgvForkInfo.getForkLiftDis());
                    moveTaskListDTO.setOperation(RgvOperation.ForkLoad.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉顶升后取货", endNode.print(), this.getClass());
                    break;
                case FORK_PUT_CARGO_AND_DOWN:
                    moveTaskListDTO.setStart_height(startNode.getForkHeight());
                    moveTaskListDTO.setEnd_height(rgvForkInfo.getForkLiftDownHeight());
                    moveTaskListDTO.setOperation(RgvOperation.ForkUnload.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉下降后放货", endNode.print(), this.getClass());
                    break;
                case FORK_RUN_WITH_CARGO:
                    moveTaskListDTO.setEnd_height(rgvForkInfo.getForkHeightRunWithCargo());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "叉车行驶后下降", endNode.print(), this.getClass());
                    break;
                case FORK_DOWN_TO_RUN:
                    moveTaskListDTO.setFork_mid_height(rgvForkInfo.getForkHeightRunWithCargo());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "叉车边行驶边下降", endNode.print(), this.getClass());
                    break;
                case FORK_UP_TO_RUN:
                    moveTaskListDTO.setFork_mid_height(endNode.getForkHeight() + rgvForkInfo.getForkLiftDis());
                    moveTaskListDTO.setOperation(RgvOperation.ForkHeight.getStrValue());
                    RgvForkLog.info(LoggerType.TASK, serialNum, "货叉行驶中升至取货完成", endNode.print(), this.getClass());
                    break;
                default:
//                    RgvForkLog.info(LoggerType.WARN, serialNum, action, "不支持的货叉动作", this.getClass());
                    break;
            }
        }
    }

    /**
     * 货叉动作
     * @param routeInfo
     * @return
     * @throws IOException
     */
    private boolean forkAction(RouteInfo routeInfo, String des) throws IOException {
        seerComm.sendRoute(routeInfo);

        String operation = routeInfo.getOperation();
        Float endHeight = routeInfo.getEnd_height();
        Float midHeight = routeInfo.getFork_mid_height();

        if (operation == null) {
            operation = routeInfo.getScript_args().getOperation();
        }

        int tick = 150;
        while (true) {
            checkRgvStatus();
            if (rgv60Info.getTaskStatus() == Rgv60TaskStatus.COMPLETED) {
                if (operation != null) {
                    if (operation.equals(RgvOperation.ForkLoad.getStrValue())
                            || operation.equals(RgvOperation.ForkUnload.getStrValue())
                            || operation.equals(RgvOperation.ForkHeight.getStrValue()))
                    {
                        if (checkForkAction(midHeight == null ? endHeight : midHeight)) {
                            break;
                        }
                    } else {
                        throw new RuntimeException("货叉动作暂时不支持该operation:" + operation);
                    }
                } else {
                    break;
                }
            }

            rgvInterrupt(des);
            if (tick > 0) {
                tick--;
            } else {
                return false;
            }
            WcsFuncUtil.delay(200);
        }

        return true;
    }

    /**
     * 检测货叉动作是否完成
     * @param height
     * @return
     */
    private boolean checkForkAction(Float height) {
        if (rgv60Info.getForkArrived()) {
            if ((height - rgvForkInfo.getForkHeightErrorRange()) <= rgv60Info.getForkHeight()
                    && (height + rgvForkInfo.getForkHeightErrorRange()) >= rgv60Info.getForkHeight()) {
                return true;
            }
        }

        return false;
    }

    /**
     * 货叉是否处于下降状态
     * @return
     */
    public boolean isForkDown() {
        if (!isOnLine()) {
            return false;
        }

        if (!rgv60Info.getForkArrived()) {
            return false;
        }

        float height = rgv60Info.getForkHeight();
        float maxDown = rgvForkInfo.getForkLiftDownHeight() + rgvForkInfo.getForkHeightErrorRange();
        float minDown = rgvForkInfo.getForkLiftDownHeight() - rgvForkInfo.getForkHeightErrorRange();
        if (height >= minDown && height <= maxDown) {
            return true;
        }

        return false;
    }

    /**
     * 获取小车信息
     * @return
     */
    public RgvForkInfoData getRgv60InfoData() {
        RgvForkInfoData rgvForkInfoData = new RgvForkInfoData();
        Rgv60InfoData rgv60InfoData = super.getRgv60InfoData();
        rgvForkInfoData.setCurrentStation(rgv60InfoData.getCurrentStation());
        rgvForkInfoData.setRelocStatus(rgv60InfoData.getRelocStatus());
        rgvForkInfoData.setRobotCurrentStation(rgv60InfoData.getRobotCurrentStation());
        rgvForkInfoData.setLastStation(rgv60InfoData.getLastStation());
        rgvForkInfoData.setSerialNum(rgv60InfoData.getSerialNum());
        rgvForkInfoData.setOnLine(rgv60InfoData.isOnLine());
        rgvForkInfoData.setRouteOnLine(rgv60InfoData.isRouteOnLine());
        rgvForkInfoData.setConfigOnLine(rgv60InfoData.isConfigOnLine());
        rgvForkInfoData.setControlOnLine(rgv60InfoData.isControlOnLine());
        rgvForkInfoData.setRobotStateOnLine(rgv60InfoData.isRobotStateOnLine());
        rgvForkInfoData.setHasRgvAuthor(rgv60InfoData.isHasRgvAuthor());
        rgvForkInfoData.setRgvStatus(rgv60InfoData.getRgvStatus());
        rgvForkInfoData.setRgvActStatus(rgv60InfoData.getRgvActStatus());
        rgvForkInfoData.setErrorInfos(rgv60InfoData.getErrorInfos());
        rgvForkInfoData.setHighestError(rgv60InfoData.getHighestError());
        rgvForkInfoData.setPalletStatus(rgv60InfoData.getPalletStatus());
        rgvForkInfoData.setPalletEnable(rgv60InfoData.getPalletEnable());
        rgvForkInfoData.setPalletActionErrorCode(rgv60InfoData.getPalletActionErrorCode());
        rgvForkInfoData.setHasCargo(rgv60InfoData.getHasCargo());
        rgvForkInfoData.setX(rgv60InfoData.getX());
        rgvForkInfoData.setY(rgv60InfoData.getY());
        rgvForkInfoData.setAngle(rgv60InfoData.getAngle());
        rgvForkInfoData.setConfidence(rgv60InfoData.getConfidence());
        rgvForkInfoData.setBlocked(rgv60InfoData.getBlocked());
        rgvForkInfoData.setBatSoc(rgv60InfoData.getBatSoc());
        rgvForkInfoData.setBatTemp(rgv60InfoData.getBatTemp());
        rgvForkInfoData.setBatVolt(rgv60InfoData.getBatVolt());
        rgvForkInfoData.setBatCur(rgv60InfoData.getBatCur());
        rgvForkInfoData.setAutoCharge(rgv60InfoData.getAutoCharge());
        rgvForkInfoData.setCharging(rgv60InfoData.getCharging());
        rgvForkInfoData.setTaskStatus(rgv60InfoData.getTaskStatus());
        rgvForkInfoData.setTargetStation(rgv60InfoData.getTargetStation());
        rgvForkInfoData.setTaskType(rgv60InfoData.getTaskType());
        rgvForkInfoData.setLoadMapStatus(rgv60InfoData.getLoadMapStatus());
        rgvForkInfoData.setMode(rgv60InfoData.getMode());
        rgvForkInfoData.setRollerMode(rgv60InfoData.getRollerMode());
        rgvForkInfoData.setCurrentMap(rgv60InfoData.getCurrentMap());
        rgvForkInfoData.setCurrentMapMD5(rgv60InfoData.getCurrentMapMD5());
        rgvForkInfoData.setWifiStrength(rgv60InfoData.getWifiStrength());
        rgvForkInfoData.setRobotName(rgv60InfoData.getRobotName());
        rgvForkInfoData.setMaps(rgv60InfoData.getMaps());
        rgvForkInfoData.setOdo(rgv60InfoData.getOdo());
        rgvForkInfoData.setTodayOdo(rgv60InfoData.getTodayOdo());
        rgvForkInfoData.setRunningTime(rgv60InfoData.getRunningTime());
        rgvForkInfoData.setCurRunningTime(rgv60InfoData.getCurRunningTime());
        rgvForkInfoData.setControlVolt(rgv60InfoData.getControlVolt());
        rgvForkInfoData.setControlTemp(rgv60InfoData.getControlTemp());
        rgvForkInfoData.setControlWet(rgv60InfoData.getControlWet());
        rgvForkInfoData.setChargeNum(rgv60InfoData.getChargeNum());
        rgvForkInfoData.setPermitMaxChgVolt(rgv60InfoData.getPermitMaxChgVolt());
        rgvForkInfoData.setPermitMaxChgCur(rgv60InfoData.getPermitMaxChgCur());
        rgvForkInfoData.setBlockX(rgv60InfoData.getBlockX());
        rgvForkInfoData.setBlockY(rgv60InfoData.getBlockY());
        rgvForkInfoData.setDriveEmc(rgv60InfoData.getDriveEmc());
        rgvForkInfoData.setHoldBrake(rgv60InfoData.getHoldBrake());
        rgvForkInfoData.setIsStop(rgv60InfoData.getIsStop());
        rgvForkInfoData.setTargetX(rgv60InfoData.getTargetX());
        rgvForkInfoData.setTargetY(rgv60InfoData.getTargetY());
        rgvForkInfoData.setTargetR(rgv60InfoData.getTargetR());
        rgvForkInfoData.setRobotNote(rgv60InfoData.getRobotNote());
        rgvForkInfoData.setForkLiftDis(rgvForkInfo.forkLiftDis);
        rgvForkInfoData.setForkLiftDownHeight(rgvForkInfo.forkLiftDownHeight);
        rgvForkInfoData.setForkHeightErrorRange(rgvForkInfo.forkHeightErrorRange);

        Map<RobotIOType.ROBOT_DI_TYPE, Boolean> DIs = rgv60Info.getDIs();
        Map<RobotIOType.ROBOT_DO_TYPE, Boolean> DOs = rgv60Info.getDOs();

        DIs.keySet().forEach(DI -> {
            rgvForkInfoData.getDI()[DI.getValue()] = DIs.get(DI);
        });

        DOs.keySet().forEach(DO -> {
            rgvForkInfoData.getDO()[DO.getValue()] = DOs.get(DO);
        });

        rgvForkInfoData.setForkHeight(rgv60Info.getForkHeightMM());
        if (rgv60Info.getForkArrived() != null) {
            rgvForkInfoData.setIsForkArrived(rgv60Info.getForkArrived() ? "到位" : "未到位");
        } else {
            rgvForkInfoData.setIsForkArrived("未知");
        }
        if (rgv60Info.getForkAutoMode() != null) {
            rgvForkInfoData.setForkMode(rgv60Info.getForkAutoMode() ? "自动" : "手动");
        } else {
            rgvForkInfoData.setForkMode("未知");
        }

        return rgvForkInfoData;
    }

    public void setForkLiftDis(Integer forkLiftDis) {
        rgvForkInfo.forkLiftDis = forkLiftDis;
    }

    public void setForkLiftDownHeight(Integer forkLiftDownHeight) {
        rgvForkInfo.forkLiftDownHeight = forkLiftDownHeight;
    }

    public void setForkHeightErrorRange(Integer forkHeightErrorRange) {
        rgvForkInfo.forkHeightErrorRange = forkHeightErrorRange;
    }
}
